/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
/**
 * \file
 * \brief Implementation for EmergencyStop interface client library.
 * \author Matthieu Myrsky <matthieu.myrsky@tkk.fi>
 *
 * $Id: EmergencyStopClient.cpp,v 1.4 2009-11-17 13:09:44 morsko Exp $
 *
 */

#include "EmergencyStopClient.hpp"
#include "EmergencyStopData.hpp"

//*****************************************************************************
using namespace MaCI::EmergencyStop;
//*****************************************************************************
//*****************************************************************************
#define GIMI_CLIENT_API (20000)
#include "gimi.h"
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************

CEmergencyStopClient::CEmergencyStopClient(gimi::GIMI *aGIMIPtr, 
                                           const int aInterfaceMinor)
  : CSync(1,1),
    CThread(2),
    CMaCI(aGIMIPtr, GIMI_PROTOCOL_MACI_EMERGENCYSTOP, aInterfaceMinor),
    iServerTimeoutTime(10000000),
    iWatchdogThreadActive(false),
    iEmergencyStopDataSequence(0),
    iLastEmergencyStopData(),
    iEmergencyStopClientCallback(NULL)

{
  dPrint(4,"CEmergencyStopClient %p constructed", this);
}
//*****************************************************************************
      
CEmergencyStopClient::~CEmergencyStopClient()
{
  DoClose();
  dPrint(4,"CEmergencyStopClient %p destructed", this);
}
//****************************************************************************

bool CEmergencyStopClient::GetEmergencyStopStatusEvent(CEmergencyStopData &aData,
                                                       unsigned int *aSeq,
                                                       const unsigned int aTimeout_ms)
{
  bool result = false;

  // Fallback in case the class is not active.
  if (!IsActive()) return false;

  Lock();
  
  if (aSeq == NULL) {
    if(iEmergencyStopDataSequence >=0){
      aData =iLastEmergencyStopData;
      result = true;
    }
  }else{
    if (iEmergencyStopDataSequence <= *aSeq) {
      int time_left = aTimeout_ms;
      unsigned int time_begin = ownTime_get_ms();

      // The available scan is older than requested - must wait for time_left
      do {
        Wait(time_left);
	time_left = aTimeout_ms - ownTime_get_ms_since(time_begin);
	
      } while(IsActive() && iEmergencyStopDataSequence < *aSeq && time_left > 0);
      
      // Now, check whether the waiting resulted in the correct sequence
      if (iEmergencyStopDataSequence > *aSeq) {
	// Yes :)
	aData = iLastEmergencyStopData;
	*aSeq = iEmergencyStopDataSequence;
	result = true;
       	
      } else {
	// No :(
	dPrint(3, "Failed to get EmergencyStopData! (Timeout %dms)",
	       aTimeout_ms);
      }
          
    }else{
      if(iEmergencyStopDataSequence > 0){
        aData = iLastEmergencyStopData;
        *aSeq = iEmergencyStopDataSequence;
        result = true;
      }
    }
  }
  Unlock();
  return result;
}
//*****************************************************************************

bool CEmergencyStopClient::ActivateClient(unsigned int aTimeout_ms)
{
  gim::binbag::CBinBag bb;
  CEmergencyStopData esData;
  CEmergencyStopData reply;
  SMessageSource esrms;
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  esData.EncodeTo(&bb);
  esData.SetCommand(KCommandActivateClient);
 
  // Send command.
  SendInterfaceMessage(esData, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(reply, esrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    const TTimeoutTime *t = reply.GetTimeoutTime();
    if(t){
      iServerTimeoutTime = t->timeoutTime_ms;
      iWatchdogThreadActive = true;
      CThread::RunThread(1); 
      return true;
    }else{
      dPrint(3,"no timeout tiem packet in reply");
      return false;
    }
  }else{
    dPrint(3,"Didn't got reply, error '%s'",GetErrorStr(e).c_str());
    return false;
  }


}
//*****************************************************************************

bool CEmergencyStopClient::DeactivateClient(unsigned int aTimeout_ms)
{
  gim::binbag::CBinBag bb;
  CEmergencyStopData esData;
  CEmergencyStopData reply;
  SMessageSource esrms;
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  esData.EncodeTo(&bb);
  esData.SetCommand(KCommandDeactivateClient);
  // Send command.
  SendInterfaceMessage(esData, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(reply, esrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    iWatchdogThreadActive = false;
    // Wait for thread to complete.
    CThread::SetRequestTermination(1);
    /* CThread::CancelThread(1); //Removed by script */
    CThread::WaitThread(1);
    return true;
  }else{
   dPrint(3,"Didn't got reply, error '%s'",GetErrorStr(e).c_str());
    return false;
  }
}
//*****************************************************************************

bool CEmergencyStopClient::ActivateEmergencyStop(unsigned int aTimeout_ms)
{
  dPrint(5,"Activate emergency stop called");
  gim::binbag::CBinBag bb;
  CEmergencyStopData esData;

  CEmergencyStopData reply;
  SMessageSource esrms;
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  esData.EncodeTo(&bb);
  esData.SetCommand(KCommandActivateEmergencyStop);
  // Send command.
  EMaCIError e = SendInterfaceMessage(esData, messageId);

  if(e != KMaCIOK){
    dPrint(3,"Failed to send interface message");
    return false;
  }
  // Wait for synchronous reply.
  e = ReceiveSynchronousMessageReply(reply, esrms, messageId, aTimeout_ms);

  if (e == KMaCIOK) {
    dPrint(6,"Got reply");
    return true;
  }else{
    dPrint(3,"Didn't got reply, error '%s'",GetErrorStr(e).c_str());
    return false;
  }
}
//*****************************************************************************

bool CEmergencyStopClient::DeactivateEmergencyStop(unsigned int aTimeout_ms)
{
gim::binbag::CBinBag bb;
  CEmergencyStopData esData;
  CEmergencyStopData reply;
  SMessageSource esrms;
  // Get Unique MessageID
  const unsigned int messageId = GetUniqueMessageID();
  
  esData.EncodeTo(&bb);
  esData.SetCommand(KCommandDeactivateEmergencyStop);
  // Send command.
  SendInterfaceMessage(esData, messageId);

  // Wait for synchronous reply.
  EMaCIError e = ReceiveSynchronousMessageReply(reply, esrms, messageId, aTimeout_ms);
  if (e == KMaCIOK) {
    
        dPrint(8,"Got reply");
    return true;
  }else{
    dPrint(3,"Didn't got reply, error '%s'",GetErrorStr(e).c_str());
    return false;
  }
}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopClient::DoOpen(void)
{
  MaCI::EMaCIError result = KMaCIError;
  
  // Checks.
  if(!IsActive()) {

    // Do Subs. using the MaCI provided function.
    result = SubscribeToInterface();

    // If all inits were fine...
    if (result == KMaCIOK) {
     
      SetActive(true);
      EnableSynchronousMessageReply();
      CThread::RunThread(0);
      dPrint(4,"CEmergencyStopClient::DoOpen() was succesfull.");
    }

  } else {
    dPrint(4,"ParameterClient already Active, not Opening again.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

MaCI::EMaCIError CEmergencyStopClient::DoClose(void)
{

  EMaCIError result = KMaCIError;
  if(IsActive()) {
    SetActive(false);

    CThread::SetRequestTermination(0);

    result = UnsubscribeToInterface(TimeoutValue(2000));

    Notify();

    CThread::WaitThread(0);
      
    if(result == KMaCIOK){
      dPrint(4,"Succesfully closed EmergencyStopClient");    
    }else{
      dPrint(4,"Unsubscribing failed at EmergencyStopClient!");
    }
  } else {
    dPrint(4, "EmergencyStopClient not active, so not Closing.");
    result = KMaCINotActive;

  }
  
  return result;
}
//*****************************************************************************

int CEmergencyStopClient::ThreadFunction(const int aThreadNumber)
{
  switch(aThreadNumber){
  case 0:{
   
    EMaCIError e;
    gimi::GIMIMessage gmsg;
    
    assert(aThreadNumber == 0);
    assert(iGIMIPtr != NULL);
    
    dPrint(4, "GIMIMessage handler started");
    
    // Run as long as the thread should be active
    while(IsActive()) {
     
      // Call the ReceiveMessage wrapper with 1s timeout
      e = ReceiveInterfaceMessage(gmsg, 1000);
      if (e == KMaCIOK) {
        dPrint(10,"Got message, process it");
        ProcessMessage(gmsg);
        
        
      } else if (e != KMaCITimeout) {
        // Other error, try to recover by sleeping one second.
        dPrint(4,"MaCIError from ReceiveInterfaceMessage? Sleeping 1s to recover..");
        ownSleep_ms(1000);
      
      }
      
    }
    
    dPrint(4, "GIMIMessage handler stopped");
    break;
  }
  case 1:{
    while(iWatchdogThreadActive){
      gim::binbag::CBinBag bb;
      CEmergencyStopData esData;
            
      esData.EncodeTo(&bb);
      esData.SetCommand(KCommandClientAlive);
      SendInterfaceMessage(esData);
      ownSleep_ms(iServerTimeoutTime/2);

    }
    break;
  }



  }
  return 0;
}
//*****************************************************************************

int CEmergencyStopClient::ProcessMessage(const gimi::GIMIMessage &msg)
{
  int result = 0;
  gim::binbag::CBinBag bb;
  CEmergencyStopData emergencyStopData;
  bool r;
  r = bb.Decode(msg.getData(), msg.getDatalength());
  if (r) {
    // Succesfully decoded as BinBag.
    
    emergencyStopData.Reset();
    r =  emergencyStopData.DecodeFrom(&bb);
    if (r) {
      dPrint(15,"data cmd is %d", emergencyStopData.GetCommand()->cmd);
      //emergencyData.Print(20);
      
      // Now Lock()
      Lock();
      // Store data to 'most-current' container.
      iLastEmergencyStopData = emergencyStopData;
      ++iEmergencyStopDataSequence;
      Unlock();
      Notify();

      // Call the handler if set.
      if (iEmergencyStopClientCallback) {
        dPrint(8,"Got Callback for EmergencyStopDataEvent - executing!");
        iEmergencyStopClientCallback->OnEmergencyStopDataEvent(emergencyStopData,
                                                     iEmergencyStopDataSequence,
                                                     *this);
        dPrint(8,"Callback for EmergencyStopDataEvent - execution complete!");
        
      }
     
    }
  }
  return result;
}
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
//*****************************************************************************
